package Detour

import (
	"math"
)

func dtIlog2(v uint) int {
	var r, shift int
	if v > 0xffff {
		r = 1 << 4
	} else {
		r = 0 << 4
	}
	v = v >> uint(r)

	if v > 0xff {
		shift = 1 << 3
	} else {
		shift = 0 << 3
	}
	v = v >> uint(shift)
	r |= shift

	if v > 0xf {
		shift = 1 << 2
	} else {
		shift = 0 << 2
	}
	v = v >> uint(shift)
	r |= shift

	if v > 0x3 {
		shift = 1 << 1
	} else {
		shift = 0 << 1
	}
	v = v >> uint(shift)
	r |= shift

	r = r | (int(v) >> 1)
	return r
}

func dtClosestPtPointTriangle(closest, p, a, b, c []float64) {
	ab := make([]float64, 3)
	ac := make([]float64, 3)
	ap := make([]float64, 3)
	dtVsub(ab, b, a)
	dtVsub(ac, c, a)
	dtVsub(ap, p, a)
	d1 := dtVdot(ab, ap)
	d2 := dtVdot(ac, ap)

	if d1 <= 0 && d2 <= 0 {
		DTVcopy(closest, a)
		return
	}
	bp := make([]float64, 3)
	dtVsub(bp, p, b)
	d3 := dtVdot(ab, bp)
	d4 := dtVdot(ac, bp)
	if d3 >= 0 && d4 <= d3 {
		DTVcopy(closest, b)
		return
	}
	vc := d1*d4 - d3*d2
	if vc <= 0 && d1 >= 0 && d3 <= 0 {
		_v := d1 / (d1 - d3)
		closest[0] = a[0] + _v*ab[0]
		closest[1] = a[1] + _v*ab[1]
		closest[2] = a[2] + _v*ab[2]
		return
	}
	// Check if P in vertex region outside C
	cp := make([]float64, 3)
	dtVsub(cp, p, c)
	d5 := dtVdot(ab, cp)
	d6 := dtVdot(ac, cp)
	if d6 >= 0.0 && d5 <= d6 {
		// barycentric coordinates (0,0,1)
		DTVcopy(closest, c)
		return
	}
	// Check if P in edge region of AC, if so return projection of P onto AC
	vb := d5*d2 - d1*d6
	if vb <= 0 && d2 >= 0 && d6 <= 0 {
		_w := d2 / (d2 - d6)
		closest[0] = a[0] + _w*ac[0]
		closest[1] = a[1] + _w*ac[1]
		closest[2] = a[2] + _w*ac[2]
		return
	}
	// Check if P in edge region of BC, if so return projection of P onto BC
	va := d3*d6 - d5*d4
	if va <= 0 && (d4-d3) >= 0 && (d5-d6) >= 0 {
		_w := (d4 - d3) / ((d4 - d3) + (d5 - d6))
		closest[0] = b[0] + _w*(c[0]-b[0])
		closest[1] = b[1] + _w*(c[1]-b[1])
		closest[2] = b[2] + _w*(c[2]-b[2])
		return
	}
	denom := 1 / (va + vb + vc)
	v := vb * denom
	w := vc * denom
	closest[0] = a[0] + ab[0]*v + ac[0]*w
	closest[1] = a[1] + ab[1]*v + ac[1]*w
	closest[2] = a[2] + ab[2]*v + ac[2]*w
}
func DTVdist(v1, v2 []float64) float64 {
	dx := v2[0] - v1[0]
	dy := v2[1] - v1[1]
	dz := v2[2] - v1[2]

	return math.Sqrt(dx*dx + dy*dy + dz*dz)
}

func DTSqr(a float64) float64 {
	return a * a
}

func DTVdist4(v1 []float64, v1Start int, v2 []float64, v2Start int) float64 {
	dx := v2[v2Start+0] - v1[v1Start+0]
	dy := v2[v2Start+1] - v1[v1Start+1]
	dz := v2[v2Start+2] - v1[v1Start+2]
	return math.Sqrt(dx*dx + dy*dy + dz*dz)
}

func DTVlerp(dest []float64, v1 []float64, v2 []float64, t float64) {
	dest[0] = v1[0] + (v2[0]-v1[0])*t
	dest[1] = v1[1] + (v2[1]-v1[1])*t
	dest[2] = v1[2] + (v2[2]-v1[2])*t
}

func DTVlerp7(dest []float64, destStart int, v1 []float64, v1Start int, v2 []float64, v2Start int, t float64) {
	dest[destStart+0] = v1[v1Start+0] + (v2[v2Start+0]-v1[v1Start+0])*t
	dest[destStart+1] = v1[v1Start+1] + (v2[v2Start+1]-v1[v1Start+1])*t
	dest[destStart+2] = v1[v1Start+2] + (v2[v2Start+2]-v1[v1Start+2])*t
}

func DtVadd(dest []float64, v1 []float64, v2 []float64) {
	dest[0] = v1[0] + v2[0]
	dest[1] = v1[1] + v2[1]
	dest[2] = v1[2] + v2[2]
}

func DtVadd6(dest []float64, destStart int, v1 []float64, v1Start int, v2 []float64, v2Start int) {
	dest[destStart+0] = v1[v1Start+0] + v2[v2Start+0]
	dest[destStart+1] = v1[v1Start+1] + v2[v2Start+1]
	dest[destStart+2] = v1[v1Start+2] + v2[v2Start+2]
}

func DTClampFloat(v, mn, mx float64) float64 {
	if v < mn {
		return mn
	} else {
		if v > mx {
			return mx
		} else {
			return v
		}
	}
}

func DTVminFloat(mn []float64, mnStart, v []float64, vStart int) {
	mn[0] = math.Min(mn[0], v[0])
	mn[1] = math.Min(mn[1], v[1])
	mn[2] = math.Min(mn[2], v[2])
}

func DTVminFloat4(mn []float64, mnStart int, v []float64, vStart int) {
	mn[mnStart+0] = math.Min(mn[mnStart+0], v[vStart+0])
	mn[mnStart+1] = math.Min(mn[mnStart+1], v[vStart+1])
	mn[mnStart+2] = math.Min(mn[mnStart+2], v[vStart+2])
}
func DTVMaxFloat(mx []float64, v []float64) {
	mx[0] = math.Min(mx[0], v[0])
	mx[1] = math.Min(mx[1], v[1])
	mx[2] = math.Min(mx[2], v[2])
}
func DTVMaxFloat64(mx []float64, mxStart int, v []float64, vStart int) {
	mx[mxStart+0] = math.Max(mx[mxStart+0], v[vStart+0])
	mx[mxStart+1] = math.Max(mx[mxStart+1], v[vStart+1])
	mx[mxStart+2] = math.Max(mx[mxStart+2], v[vStart+2])
}

func dtOverlapBounds(amin []float64, amax []float64, bmin []float64, bmax []float64) bool {
	overlap := true
	if amin[0] > bmax[0] || amax[0] < bmin[0] {
		overlap = false
	}

	if amin[1] > bmax[1] || amax[1] < bmin[1] {
		overlap = false
	}

	if amin[2] > bmax[2] || amax[2] < bmin[2] {
		overlap = false
	}
	return overlap
}

func dtOverlapQuantBounds(amin []uint16, amax []uint16, bmin []uint16, bmax []uint16) bool {
	overlap := true
	if (amin[0] > bmax[0]) || (amax[0] < bmin[0]) {
		overlap = false
	}

	if (amin[1] > bmax[1]) || (amax[1] < bmin[1]) {
		overlap = false
	}

	if (amin[2] > bmax[2]) || (amax[2] < bmin[2]) {
		overlap = false
	}

	return overlap
}

func dtIntersectSegmentPoly2D(p0, p1, verts []float64, nverts int, tmin, tmax *float64, segMin, segMax *int) bool {
	EPS := 0.00000001
	*tmin = 0
	*tmax = 1
	*segMax = -1
	*segMin = -1

	dir := make([]float64, 3)
	dtVsub(dir, p1, p0)

	j := nverts - 1
	for i := 0; i < nverts-1; i++ {
		edge := make([]float64, 3)
		diff := make([]float64, 3)
		dtVsub6(edge, 0.0, verts, i*3, verts, j*3)
		dtVsub6(diff, 0.0, p0, 0.0, verts, j*3)
		n := dtVperp2D(edge, diff)
		d := dtVperp2D(dir, edge)
		if math.Abs(d) < EPS {
			if n < 0 {
				return false
			} else {
				continue
			}
		}
		t := n / d
		if d < 0 {
			if t > *tmin {
				*tmin = t
				*segMin = j
				if *tmin > *tmax {
					return false
				}
			}
		} else {
			if t < *tmax {
				*tmax = t
				*segMax = j
				if *tmax < *tmin {
					return false
				}
			}
		}
		j = i
	}
	return true
}

func DTDistancePtSegSqr2D(pt []float64, ptStart int, p []float64, pStart int, q []float64, qStart int, t *float64) float64 {
	pqx := q[qStart+0] - p[pStart+0]
	pqz := q[qStart+2] - p[pStart+2]
	dx := pt[ptStart+0] - p[pStart+0]
	dz := pt[ptStart+2] - p[pStart+2]
	d := pqx*pqx + pqz*pqz
	*t = pqx*dx + pqz*dz
	if d > 0 {
		*t /= d
	}
	if *t < 0 {
		*t = 0
	} else if *t > 1 {
		*t = 1
	}
	dx = p[pStart+0] + *t*pqx - pt[ptStart+0]
	dz = p[pStart+2] + *t*pqz - pt[ptStart+2]
	return dx*dx + dz*dz
}

func dtCalcPolyCenter(tc []float64, idx []uint16, nidx int, verts []float64) {
	tc[0], tc[1], tc[2] = 0, 0, 0
	for j := 0; j < nidx; j++ {
		vIndex := idx[j] * 3
		tc[0] += verts[vIndex+0]
		tc[1] += verts[vIndex+1]
		tc[2] += verts[vIndex+2]
	}
	s := float64(1.0 / nidx)
	tc[0] *= s
	tc[1] *= s
	tc[2] *= s
}

func dtSqr(a float64) float64 {
	return a * a
}

func dtOppositeTile(side int) int {
	return (side + 4) & 0x7
}

func DTSwapFloat(lhs, rhs *float64) {
	temp := *lhs
	*lhs = *rhs
	*rhs = temp
}

func dtVlenSqr(v []float64) float64 {
	return v[0]*v[0] + v[1]*v[1] + v[2]*v[2]
}

func dtClosestHeightPointTriangle(p []float64, pStart int, a []float64, aStart int, b []float64, bStart int, c []float64, cStart int, h *float64) bool {
	v0 := make([]float64, 3)
	v1 := make([]float64, 3)
	v2 := make([]float64, 3)
	dtVsub6(v0, 0, c, cStart, a, aStart)
	dtVsub6(v1, 0, b, bStart, a, aStart)
	dtVsub6(v2, 0, p, pStart, a, aStart)

	dot00 := dtVdot2D(v0, v0)
	dot01 := dtVdot2D(v0, v1)
	dot02 := dtVdot2D(v0, v2)
	dot11 := dtVdot2D(v1, v1)
	dot12 := dtVdot2D(v1, v2)

	invDenom := 1.0 / (dot00*dot11 - dot01*dot01)
	u := (dot11*dot02 - dot01*dot12) * invDenom
	v := (dot00*dot12 - dot01*dot02) * invDenom

	const EPS = 1e-4
	if u >= -EPS && v >= -EPS && (u+v) <= 1+EPS {
		*h = a[aStart+1] + v0[1]*u + v1[1]*v
		return true
	}
	return false
}

func dtPointInPolygon(pt []float64, verts []float64, nverts int) bool {
	i, j := 0, 0
	c := false
	for i = 0; i < nverts; i++ {
		viIndex := i * 3
		vjIndex := j * 3

		if (verts[viIndex+2] > pt[2]) != (verts[vjIndex+2] > pt[2]) &&
			(pt[0] < (verts[vjIndex+0]-verts[viIndex+0])*(pt[2]-verts[viIndex+2])/(verts[vjIndex+2]-verts[viIndex+2])+verts[viIndex+0]) {
			c = !c
		}
		j = i
	}
	return c
}

func dtDistancePtPolyEdgesSqr(pt []float64, ptStart int, v []float64, nverts int, ed []float64, et []float64) bool {
	var j int
	c := false
	j = nverts - 1
	for i := 0; i < nverts; i++ {
		vi := i * 3
		vj := j * 3
		if (v[vi+2] > pt[ptStart+2]) != (v[vj+2] > pt[ptStart+2]) &&
			pt[ptStart+0] < (v[vj+0]-v[vi+0])*(pt[ptStart+2]-v[vi+2])/(v[vj+2]-v[vi+2])+v[vi+0] {
			c = !c
		}
		ed[j] = DTDistancePtSegSqr2D(pt, ptStart, v, vj, v, vi, &et[j])
		j = i
	}
	return c
}

func dtVsub(dest, v1, v2 []float64) {
	dest[0] = v1[0] - v2[0]
	dest[1] = v1[1] - v2[1]
	dest[2] = v1[2] - v2[2]
}

func dtVsub6(dest []float64, destStart int, v1 []float64, v1Start int, v2 []float64, v2Start int) {
	dest[destStart+0] = v1[v1Start+0] - v2[v2Start+0]
	dest[destStart+1] = v1[v1Start+1] - v2[v2Start+1]
	dest[destStart+2] = v1[v1Start+2] - v2[v2Start+2]
}

func dtVdot(v1, v2 []float64) float64 {
	return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]
}

func dtVdot4(v1, v2 []float64, v1Start, v2Start int) float64 {
	return v1[v1Start+0]*v2[v2Start+0] + v1[v1Start+1]*v2[v2Start+1] + v1[v1Start+2]*v2[v2Start+2]
}

func dtVdot2D(u []float64, v []float64) float64 {
	return u[0]*v[0] + u[2]*v[2]
}

func dtVdot2D4(u []float64, uStart int, v []float64, vStart int) float64 {
	return u[uStart+0]*v[vStart+0] + u[uStart+2]*v[vStart+2]
}

func DTVcopy(dest, a []float64) {
	dest[0] = a[0]
	dest[1] = a[1]
	dest[2] = a[2]
}

func dtVcopy4(dest []float64, destStart int, a []float64, aStart int) {
	dest[destStart+0] = a[aStart+0]
	dest[destStart+1] = a[aStart+1]
	dest[destStart+2] = a[aStart+2]
}
func dtVperp2D(u, v []float64) float64 {
	return u[2]*v[0] - u[0]*v[2]
}
func dtVperp2D4(u []float64, uStart int, v []float64, vStart int) float64 {
	return u[uStart+2]*v[vStart+0] - u[uStart+0]*v[vStart+2]
}

func DTNextPow2(v uint) uint {
	v--
	v |= v >> 1
	v |= v >> 2
	v |= v >> 4
	v |= v >> 8
	v |= v >> 16
	v++
	return v
}

func DTTriArea2D(a, b, c []float64) float64 {
	abx := b[0] - a[0]
	abz := b[2] - a[2]
	acx := c[0] - a[0]
	acz := c[2] - a[2]
	return acx*abz - abx*acz
}

func DTVequal(p0, p1 []float64) bool {
	thrSqrt := 1.0 / 16384.0
	thr := thrSqrt * thrSqrt
	d := DTVdistSqr(p0, p1)
	return d < thr
}

func DTVdistSqr(v1, v2 []float64) float64 {
	dx := v2[0] - v1[0]
	dy := v2[1] - v1[1]
	dz := v2[2] - v1[2]
	return dx*dx + dy*dy + dz*dz
}
